
#include "CRC.h"
#include "TW.h"
#include <wx/ctb/serport.h>

extern wxSerialPort *pDevice;

void SendData(unsigned char *pData,int Len)
{
	pDevice->Write((char*)pData,Len);
//    BC4_Send((char*)pData,Len);
}

int GetDataTimed(unsigned char *pData,int Len,int TimeOut)
{
	int Tot = 0,Time = 0;
    int Now;
    int Begin = GetTickCount();
    Now = Begin;
    while(Tot < Len && TimeOut > (Now-Begin))
    {
		if(pDevice->Read((char*)pData,1) == 1)
        {
            Tot++;
            pData++;
            Begin = GetTickCount();//Se iniziano ad arrivare dati riazzero il timeout
        }else
        {
            Sleep(2);
        }
        Now = GetTickCount();
    }	
    return Tot;
//    BC4_Send((char*)pData,Len);
}


int VerifyCRC(unsigned char *pData,int Len)
{
	unsigned short CRC;
	unsigned short CRC16 = CrcCreate(&pData[6],Len-8);
	memcpy(&CRC,&pData[Len-2],sizeof(CRC));
	return (CRC == CRC16)?1:0;
}


int StuffData(unsigned char *pUnstuffed,int Len,unsigned char *pStuffed)
{
    int i;
	unsigned char Curr;
	unsigned char *pOut = pStuffed;
	for(i=0;i<Len;i++)
	{
		Curr = *pUnstuffed++;
		if(Curr == ESCAPE_FRAME || Curr == BEGIN_FRAME || Curr == END_FRAME)
		{
			*pOut++ = ESCAPE_FRAME;
			*pOut++ = Curr-ESCAPE_FRAME;
		}else
		{
			*pOut++ = Curr;
		}
	}
	return (int)(pOut - pStuffed);
}
int UnstuffData(unsigned char *pData,int Len,unsigned char *pUnstuffed)
{
	unsigned char *pLast = &pData[Len];
	unsigned char *pOut = pUnstuffed;
	while(pData<pLast)
	{
		if(*pData	!= ESCAPE_FRAME)
		{
			*pOut++ = *pData++;
		}else
		{
			pData++;
			*pOut++ = ESCAPE_FRAME +*pData++;
		}
	}
	return (int)(pOut - pUnstuffed);
}



int SendFrame(unsigned char *PayLoad,int Len,int *pFrameNumber)
{
	unsigned short CRC;
	unsigned char Temp[PACKET_LEN],OutBuffer[PACKET_LEN];
	short int FrameLen;
	
	FrameLen = 8 + Len;
	memcpy(&Temp[0],&FrameLen,sizeof(FrameLen));
	memcpy(&Temp[2],pFrameNumber,sizeof(int));
	(*pFrameNumber)++;
	memcpy(&Temp[6],PayLoad,Len);
	CRC = CrcCreate(&Temp[6],Len);
	memcpy(&Temp[FrameLen-2],&CRC,sizeof(CRC));

	FrameLen = StuffData(Temp,FrameLen,&OutBuffer[1]);
	OutBuffer[0] = BEGIN_FRAME;
	FrameLen+=2;
	OutBuffer[FrameLen-1] = END_FRAME;

    SendData(OutBuffer,FrameLen);

	return FrameLen;
}

int DecodeFrame(unsigned char *pData,short int Len,unsigned char *PayLoad)
{
	unsigned short int FrameLen;
	int Begin=0,End=0;

	unsigned char Unstuffed[PACKET_LEN];
	int	UnstuffedLen;

	if(Len<2) return 0;

	
	for(Begin = 0;Begin < Len; Begin++)
	{
		if(pData[Begin] == BEGIN_FRAME)break;
	}
	for(End = Begin;End < Len; End++)
	{
		if(pData[End] == END_FRAME)break;
	}
	Len = End -Begin -1;
	if(End <= Begin && Len<=0) return 0;
		
	UnstuffedLen = UnstuffData(&pData[Begin+1],Len,Unstuffed);
	
	memcpy(&FrameLen,Unstuffed,sizeof(FrameLen));
	if(FrameLen != UnstuffedLen)
	{
		return 0;
	}
	if(VerifyCRC(Unstuffed,UnstuffedLen))
	{
		memcpy(PayLoad,&Unstuffed[6],FrameLen-8);
		//Mettere decrypt
		return FrameLen-8;
	}else
	{//Error Management
		//No answer
		return 0;	
	}
}

int RecvFrame(unsigned char *pData)
{
	int Received=0;
	unsigned char Temp;
	bool Started=FALSE;
	int Elapsed, Begin = GetTickCount();
	do{
		//Len = GetDataTimed(Buffer,1,512);
		//pDev->Read((char*)pData,1) == 1
		//for(i=0;i<Len;i++)
		if(pDevice->Read((char*)&Temp,1) == 1)
		{
			//Temp = Buffer[i];
			switch(Temp)
			{
			case BEGIN_FRAME:
				
				pData[Received] = Temp;
				Received++;
				Started = TRUE;
				break;
			case END_FRAME:
				Started = FALSE;
				pData[Received] = Temp;
				Received++;
				return Received;
			default:
				if(Started)
				{
					pData[Received] = Temp;
					Received++;
					if(Received >= PACKET_LEN) 
					{
						//Log(_("Receive buffer overflow"));
						Received = 0;
						Started = FALSE;
					}
				}
				break;
			}
		}else
		{
			Sleep(2);		
		}
		Elapsed = GetTickCount() - Begin;
	}while(Elapsed < TIMEOUT);
	return 0;
} 


bool ReceiveCommand(TW_Command	*pCommand)
{
	unsigned char	TempData[PACKET_LEN],Payload[PACKET_LEN];
	int Len;
	
	//Sleep(PrePause);
	Len = RecvFrame(TempData);//,END_FRAME,PrePause);
	if(Len >0)
	{
		Len = DecodeFrame(TempData,Len,Payload);
		memset(pCommand,0,sizeof(TW_Command));
		if(Len == sizeof(TW_Command) && Payload[0] == TW_COMMAND)
		{
			memcpy(pCommand,Payload,sizeof(TW_Command));
			return TRUE;
		}
	}
	return FALSE;
}
bool ReceiveObjData(TW_ObjData	*pData)
{
	unsigned char	TempData[PACKET_LEN],Payload[PACKET_LEN];
	int Len;
	
	//Sleep(PrePause);
	Len = RecvFrame(TempData);//,END_FRAME,PrePause);
	if(Len >0)
	{
		Len = DecodeFrame(TempData,Len,Payload);
		memset(pData,0,sizeof(TW_ObjData));
		if(Payload[0] == TW_DATA)
		{

			memcpy(pData,Payload,sizeof(TW_ObjData)-MAX_BLOCKLEN);
			if(pData->DataLen <= MAX_BLOCKLEN)
			{
				memcpy(pData->Data,&Payload[sizeof(TW_ObjData)-MAX_BLOCKLEN],pData->DataLen);
				return TRUE;
			}
		}
	}
	return FALSE;
}
bool ReceiveStreamData(TW_StreamingData	*pData)
{
	unsigned char	TempData[PACKET_LEN],Payload[PACKET_LEN];
	int Len;
	
	//Sleep(PrePause);
	Len = RecvFrame(TempData);//,END_FRAME,PrePause);
	if(Len >0)
	{
		Len = DecodeFrame(TempData,Len,Payload);
		memset(pData,0,sizeof(TW_StreamingData));
		if(Payload[0] == TW_STARTSYNTH)
		{
			memcpy(pData,Payload,sizeof(TW_StreamingData)-MAX_BLOCKLEN*2);
			if(pData->BlockLen <= MAX_BLOCKLEN)
			{
				memcpy(pData->Data,&Payload[sizeof(TW_StreamingData)-MAX_BLOCKLEN*2],pData->BlockLen*2);
				return TRUE;
			}
		}
	}
	return FALSE;
}
void    SendObjData(TW_ObjData *pData,int FrameNumber)
{
    SendFrame((unsigned char*)pData,pData->DataLen+12,&FrameNumber);
}

void    SendCommand(TW_Command *pData,int FrameNumber)
{
    SendFrame((unsigned char*)pData,sizeof(TW_Command),&FrameNumber);
}

